#include <functional>
#include <iostream>
#include <thread>
#include <list>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <unistd.h>
#include <sys/time.h> 
#include <arpa/inet.h>	/* inet_addr */

#include "xlinkHS.h"
#include "rtk.h"

/**
  ******************************************************************************
  * @file           : xlinkHS.c
  * @brief          : implement for xlinkHS.c file.
  *                   This file contains the common implement of the application.
  * @author			: ac
  * @attention
  *
  *		Copyright (c) 2023 Technology Co., Ltd.
  *
  *******************************************************************************
  */


/*
 * globla variable
 */


int client_fd = -1;
struct sockaddr_in ser_addr;
struct timeval ta;


static const XAG_U16 crc16tab[256]= {
    0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7,
    0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef,
    0x1231,0x0210,0x3273,0x2252,0x52b5,0x4294,0x72f7,0x62d6,
    0x9339,0x8318,0xb37b,0xa35a,0xd3bd,0xc39c,0xf3ff,0xe3de,
    0x2462,0x3443,0x0420,0x1401,0x64e6,0x74c7,0x44a4,0x5485,
    0xa56a,0xb54b,0x8528,0x9509,0xe5ee,0xf5cf,0xc5ac,0xd58d,
    0x3653,0x2672,0x1611,0x0630,0x76d7,0x66f6,0x5695,0x46b4,
    0xb75b,0xa77a,0x9719,0x8738,0xf7df,0xe7fe,0xd79d,0xc7bc,
    0x48c4,0x58e5,0x6886,0x78a7,0x0840,0x1861,0x2802,0x3823,
    0xc9cc,0xd9ed,0xe98e,0xf9af,0x8948,0x9969,0xa90a,0xb92b,
    0x5af5,0x4ad4,0x7ab7,0x6a96,0x1a71,0x0a50,0x3a33,0x2a12,
    0xdbfd,0xcbdc,0xfbbf,0xeb9e,0x9b79,0x8b58,0xbb3b,0xab1a,
    0x6ca6,0x7c87,0x4ce4,0x5cc5,0x2c22,0x3c03,0x0c60,0x1c41,
    0xedae,0xfd8f,0xcdec,0xddcd,0xad2a,0xbd0b,0x8d68,0x9d49,
    0x7e97,0x6eb6,0x5ed5,0x4ef4,0x3e13,0x2e32,0x1e51,0x0e70,
    0xff9f,0xefbe,0xdfdd,0xcffc,0xbf1b,0xaf3a,0x9f59,0x8f78,
    0x9188,0x81a9,0xb1ca,0xa1eb,0xd10c,0xc12d,0xf14e,0xe16f,
    0x1080,0x00a1,0x30c2,0x20e3,0x5004,0x4025,0x7046,0x6067,
    0x83b9,0x9398,0xa3fb,0xb3da,0xc33d,0xd31c,0xe37f,0xf35e,
    0x02b1,0x1290,0x22f3,0x32d2,0x4235,0x5214,0x6277,0x7256,
    0xb5ea,0xa5cb,0x95a8,0x8589,0xf56e,0xe54f,0xd52c,0xc50d,
    0x34e2,0x24c3,0x14a0,0x0481,0x7466,0x6447,0x5424,0x4405,
    0xa7db,0xb7fa,0x8799,0x97b8,0xe75f,0xf77e,0xc71d,0xd73c,
    0x26d3,0x36f2,0x0691,0x16b0,0x6657,0x7676,0x4615,0x5634,
    0xd94c,0xc96d,0xf90e,0xe92f,0x99c8,0x89e9,0xb98a,0xa9ab,
    0x5844,0x4865,0x7806,0x6827,0x18c0,0x08e1,0x3882,0x28a3,
    0xcb7d,0xdb5c,0xeb3f,0xfb1e,0x8bf9,0x9bd8,0xabbb,0xbb9a,
    0x4a75,0x5a54,0x6a37,0x7a16,0x0af1,0x1ad0,0x2ab3,0x3a92,
    0xfd2e,0xed0f,0xdd6c,0xcd4d,0xbdaa,0xad8b,0x9de8,0x8dc9,
    0x7c26,0x6c07,0x5c64,0x4c45,0x3ca2,0x2c83,0x1ce0,0x0cc1,
    0xef1f,0xff3e,0xcf5d,0xdf7c,0xaf9b,0xbfba,0x8fd9,0x9ff8,
    0x6e17,0x7e36,0x4e55,0x5e74,0x2e93,0x3eb2,0x0ed1,0x1ef0
};

/*
 * function declaration
 */
XAG_U16 Utils_crc16_ccitt(const XAG_U8 *buf, XAG_U32 len);
XAG_U16 crc_ccitt2(XAG_U8 * input1, int len1, XAG_U8 * input2, int len2);
XAG_U16 crc_ccitt3(XAG_U8 * input1, int len1, XAG_U8 * input2, int len2, XAG_U8 * input3, int len3);



/**
 * Utils_crc16_ccitt - crc function.
 * @buf		: buffer
 * @len		: buffer lenght
 */
XAG_U16 Utils_crc16_ccitt(const XAG_U8 *buf, XAG_U32 len)
{
    uint32_t counter;
    XAG_U16 crc = 0xFFFF;

    for( counter = 0; counter < len; counter++) {
        crc = (crc << 8) ^ crc16tab[((crc >> 8) ^ *(XAG_U8 *)buf++) & 0x00FF];
    }

    return ~crc;
}

XAG_U16 crc_ccitt2(XAG_U8 * input1, int len1, XAG_U8 * input2, int len2)
{
    XAG_U16 crc = 0xFFFF;

    while (len1-- > 0)
        crc = crc16tab[(crc >> 8 ^ * input1++) & 0xff] ^ (crc << 8);

    while (len2-- > 0)
        crc = crc16tab[(crc >> 8 ^ * input2++) & 0xff] ^ (crc << 8);

    return ~crc;
}

XAG_U16 crc_ccitt3(XAG_U8 * input1, int len1, XAG_U8 * input2, int len2, XAG_U8 * input3, int len3)
{
    XAG_U16 crc = 0xFFFF;

    while (len1-- > 0)
        crc = crc16tab[(crc >> 8 ^ * input1++) & 0xff] ^ (crc << 8);

    while (len2-- > 0)
        crc = crc16tab[(crc >> 8 ^ * input2++) & 0xff] ^ (crc << 8);

	while (len3-- > 0)
		crc = crc16tab[(crc >> 8 ^ * input3++) & 0xff] ^ (crc << 8);      
        
    return ~crc;
}

void package_0x80(unsigned char *payload, short payload_len)
{
	XlinkHS_0x80_Send_Head_T send = {0};
	char send_buff[1024] = {0};
	int i = 0;
	int checkSum = 0;
	int send_len = 0;
	unsigned char test[8] = {0x82, 0x01, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00};
    //unsigned char test[8] = {0x82, 0x00, 0x04, 0x00, 0x04, 0x00, 0x00, 0x00};
    //unsigned char test[8] = {0x82, 0x01, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00};

	send.u8Prefix = 0x80;
	send.u8Retry = 0;
	send.u16Length = (payload_len+2) | ((payload_len+2) >> 8);
	//send.u32Address = (14 & 0xff) | ((0 & 0xff) << 8) | 
	//				  ((0 & 0xff) << 16) | ((10 & 0xff) << 24);	/* 10.0.0.14 */
	send.u32Address = (36 & 0xff) | ((0 & 0xff) << 8) | 
					  ((31 & 0xff) << 16) | ((172 & 0xff) << 24);	/* 172.31.0.36 */
	send.u16DataType = 0x0000;
	send.u8HwPort = 0x0;		/* lan to usb_cdc */
	send.u8PacketType = 0;
	send.u8LocalPort = 0;
	send.u8Version = 1;
	send.u16ToPort = 0x08;		/* USB_CDC */
	send.u16FromPort = 0x2;		/* LAN */
	checkSum = crc_ccitt2((unsigned char*)&send, sizeof(XlinkHS_0x80_Send_Head_T), payload, payload_len);
	
	memcpy(send_buff, (char *)&send, sizeof(XlinkHS_0x80_Send_Head_T));
	memcpy(send_buff + sizeof(XlinkHS_0x80_Send_Head_T), payload, payload_len);
	memcpy(send_buff + sizeof(XlinkHS_0x80_Send_Head_T) + payload_len, &checkSum, 2);

	send_len = sizeof(XlinkHS_0x80_Send_Head_T) + payload_len + 2;
	for (i = 0; i < send_len; i++) {
		printf("%.2x ",(unsigned char)send_buff[i]);
	}
	printf("\n");

	gettimeofday( &ta, NULL );

	/* tcp send */
	//tcp_send(send_buff, send_len);

	/* udp send */
	//udp_send_msg(client_fd, (struct sockaddr*)&ser_addr, (unsigned char *)test, 8);
    //sleep(1);
    udp_send_msg(client_fd, (struct sockaddr*)&ser_addr, (unsigned char *)send_buff, send_len);
	//udp_send_data(client_fd, RTK_UDP_SERVER_IP, RTK_UDP_SERVER_PORT, (void *)send_buff, send_len);
	//udp_send_data(client_fd, RTK_UDP_SERVER_IP, RTK_UDP_SERVER_PORT, (void *)test, 8);
}

/**
 * udp_init - udp init function.
 * @none
 */
int udp_init(void)
{
	pthread_attr_t attr;
	pthread_t rid; 

    client_fd = socket(AF_INET, SOCK_DGRAM, 0);
    if(client_fd < 0)
    {
        printf("create socket fail!\n");
        return -1;
    }
	printf("udp client fd:%d\n", client_fd);

    memset(&ser_addr, 0, sizeof(ser_addr));
    ser_addr.sin_family = AF_INET;
    ser_addr.sin_addr.s_addr = inet_addr(RTK_UDP_SERVER_IP);
    ser_addr.sin_port = htons(RTK_UDP_SERVER_PORT); 

	pthread_attr_init(&attr);
	pthread_create(&rid, &attr, (void * (*)(void *))udp_recv, NULL);

    //close(client_fd);

    return 0;
}

bool udp_send_data(SOCKET SocketFd,const char *pSerIP,unsigned short Port,void *pBuff, int len)
{
	struct sockaddr_in	server;
	int i = 0;

	if(SocketFd < 0) 
		return -1;

	//memset(&server,0,sizeof(server));
	server.sin_family = AF_INET;
	server.sin_addr.s_addr = inet_addr(pSerIP);
	server.sin_port = htons((unsigned short)Port);

	return (sendto(SocketFd,(const char *)pBuff, len, 0, (struct sockaddr*)&server, sizeof(server)) != -1) ? true : false;
}

void udp_send_msg(int fd, struct sockaddr* dst, unsigned char * buf, int b_len)
{
    socklen_t len;
	int i = 0;

	len = sizeof(*dst);	
	sendto(fd, buf, b_len, 0, dst, len);
	
	#if 0
	printf("send:\n");
	for (i = 0; i < b_len; i++) {
		printf("%.2x ", buf[i]);
	}
	printf("\nend:\n");	// 去掉会延时输出，需找找原因
	#endif
}

void handle_udp_msg(int fd)
{
    unsigned char buf[BUFF_LEN];  //接收缓冲区，1024字节
    socklen_t len;
    int count = 0, i = 0;
    struct sockaddr_in clent_addr;  //clent_addr 用于记录发送方的地址信息
	struct timeval start, end;
	float  timeuse;

    while(1)
    {
        memset(buf, 0, BUFF_LEN);
        len = sizeof(clent_addr);

		// 拍照
		//system("camera_if_direct 0x0 0xb 0x3");

		gettimeofday( &start, NULL );
        count = recvfrom(fd, buf, BUFF_LEN, 0, (struct sockaddr*)&clent_addr, &len);  //recvfrom是拥塞函数，没有数据就一直拥塞
        if(count == -1) 
        {
            printf("recieve data fail!\n");
            return;
        }
		gettimeofday( &end, NULL );

		timeuse = 1000000 * ( end.tv_sec - start.tv_sec ) + (end.tv_usec - start.tv_usec); 
		timeuse /= 1000;
		printf("--- recvfrom timeuse = %f\n", timeuse);
		
		timeuse = 0.0;
		timeuse = 1000000 * ( end.tv_sec - ta.tv_sec ) + (end.tv_usec - ta.tv_usec); 
		timeuse /= 1000;
		printf("--- send->recv timeuse = %f\n", timeuse);

		#if 0
        printf("------------- %s -----------\n", "begin");
        for (i = 0; i < count; i++) {
            printf("%.2x ",buf[i]);
        }
        printf("\n------------- %s -----------\n", "end");
		#endif

		analysis(buf, count);
		
		usleep(50000);
    }
}

void * udp_recv(void *arg)
{
	handle_udp_msg(client_fd);   

	return 0;
}

void udp_uninit()
{
	close(client_fd);
}

void * tcp_recv(void *arg)
{
	char recv_buff[BUFF_LEN] = {0};
	int count = 0, i = 0;
    
    while(1) {
		recv(client_fd, recv_buff, sizeof(recv_buff)-1, 0);

		printf("------------- %s -----------\n", "begin");
        for (i = 0; i < count; i++) {
            printf("%.2x ",recv_buff[i]);
        }
        printf("------------- %s -----------\n", "end");

		usleep(50000);
    }    
}

int tcp_send(char *psend, int len)
{
	int count = 0;

	count = send(client_fd, psend, len + 1, 0);

	return count;
}

int tcp_init()
{
	pthread_attr_t attr;
	pthread_t rid;   

    client_fd = socket(AF_INET, SOCK_STREAM, 0);
	printf("tcp client fd:%d\n", client_fd);

    struct sockaddr_in serv_addr;
    memset(&serv_addr, 0, sizeof(serv_addr));  
    serv_addr.sin_family = AF_INET;  
    serv_addr.sin_addr.s_addr = inet_addr("172.31.0.1");  
    serv_addr.sin_port = htons(10087);  

	pthread_attr_init(&attr);
    
    while (connect(client_fd, (struct sockaddr*)&serv_addr, sizeof(serv_addr))) {
        sleep(1);
        printf("wait serve connect\r\n");
    }    
         
    pthread_create(&rid, &attr, (void * (*)(void *))tcp_recv, NULL);
    
    return 0;
}

void tcp_uninit()
{
	close(client_fd);
}

static void SetSystemClock(unsigned long long utc_time)
{
    struct timeval tv ;

    tv.tv_sec = (utc_time/1000)+2*8*3600;
    tv.tv_usec = (utc_time%1000)*1000;		/* convert to us */
 
    settimeofday(&tv, NULL);
}

static struct timeval GetMonotonic()
{
    struct timespec stTime;

    clock_gettime(CLOCK_MONOTONIC, &stTime);

    struct timeval stRet;

    stRet.tv_sec = stTime.tv_sec - timezone;
    stRet.tv_usec = stTime.tv_nsec/1000;

    return stRet;
}

void test_GetMonotonic()
{
	struct timeval tmData;
    unsigned long long system_time;

	tmData = GetMonotonic();
    system_time = tmData.tv_sec*1000 + tmData.tv_usec / 1000;
}

static unsigned long long GetSystemClock()
{
    struct timeval tv;
	unsigned long long tv_sen = 0, gltime = 0;

    gettimeofday(&tv, NULL);

    tv_sen = tv.tv_sec;
    gltime = tv_sen * 1000 + tv.tv_usec / 1000;

    return gltime;
}

static unsigned long long  GetSystemClock_ns()
{
	struct timespec time_start, time_end;

	clock_gettime(CLOCK_REALTIME, &time_start);

	return (long long unsigned)time_start.tv_sec * 1000000000 + (long long unsigned)time_start.tv_nsec;
}

int Convert_utc_time( unsigned long long utc, char * ptime)
{
	struct tm *tm;
	time_t t = 0;

	if (utc == 0) {
		strncpy(ptime, "no time", 8);
		return -1;
	}
	
	t = utc / 1000;
	tm = gmtime(&t);
	sprintf(ptime, "%04d:%02d:%02d %02d:%02d:%02d.%03lld", tm->tm_year+1900, tm->tm_mon+1, tm->tm_mday,
	tm->tm_hour, tm->tm_min, tm->tm_sec, utc%1000);

	printf("utc time for ac debug : [%s]\n", ptime);

	return 0;
}

void analysis(unsigned char * pdata, int len)
{
	unsigned int reply0x80headLen = sizeof(XlinkHS_0x80_Return_T);
	unsigned int rtkDataHeadSize = sizeof(Rtk_0x5B_Header_T);
	int i = 0;
	Rtk_0x5B_Header_T *p_rtk = (Rtk_0x5B_Header_T *)&pdata[reply0x80headLen];

	printf("------------- %s -----------\n", "begin");
	for (i = 0; i < len; i++) {
		printf("%.2x ",pdata[i]);
	}
	printf("\n------------- %s -----------\n", "end");

	if (p_rtk->u8Cmd == CMD_RTK_BASE_STATION_STATUS) {
		rtk_resolving_0x02_cmd(&pdata[reply0x80headLen + rtkDataHeadSize], sizeof(Station_Status_T));
	}
	else if (p_rtk->u8Cmd == CMD_RTK_EVENT_TRIGGER) {
		rtk_resolving_0x25_cmd(&pdata[reply0x80headLen + rtkDataHeadSize], 80);
	}
}

